import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data

from sensor_msgs.msg import JointState
from message_filters import ApproximateTimeSynchronizer, Subscriber

class JointRelayApp(Node):
    def __init__(self):
        super().__init__('joint_states_relay_node')
        
        self.__arm_sub = Subscriber(self, JointState, "/usv/arm/joint_states")
        self.__hand_sub = Subscriber(self, JointState, "/usv/arm/gripper/joint_states", qos_profile=qos_profile_sensor_data)

        self.__all_pub = self.create_publisher(JointState, 'joint_states', 10)

        queue_size = 30
        self.__tss = ApproximateTimeSynchronizer(
            [self.__arm_sub, self.__hand_sub],
            queue_size,
            0.01,  # defines the delay (in seconds) with which messages can be synchronized
        )
        self.__tss.registerCallback(self.__message_callback)
    
    def __message_callback(self, arm_states, hand_states):
        all_states = JointState()

        all_states.header = arm_states.header
        all_states.name = arm_states.name + hand_states.name + ['finger_tip_left_joint', 'finger_tip_right_joint']
        
        all_states.position = arm_states.position + hand_states.position
        all_states.velocity = arm_states.velocity + hand_states.velocity
        all_states.effort = arm_states.effort + hand_states.effort
        for _ in range(2):
            all_states.position.append(0.0)
            all_states.velocity.append(0.0)
            all_states.effort.append(0.0)

        self.__all_pub.publish(all_states)

def main(args=None):
    rclpy.init(args=args)

    node = JointRelayApp()

    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()


